//
//  ukf.h
//  KFilter
//
//  Created by Kaiwen Gu on 6/23/13.
//  Copyright (c) 2013 com.mika. All rights reserved.
//
#include "arm_math.h"

#ifndef KFilter_ukf_h
#define KFilter_ukf_h

#define MATRIX_DEBUG 0
#define debug_mprint(name, args ...) if (MATRIX_DEBUG) mprintf(name, args)

#define ONE_CR 1
#define MAX_OBS_LENGTH 3

typedef void ukf_predictionUpdate(float32_t * inState, float32_t * outState, float32_t * noise, float32_t * exogenousInput, float32_t dt);
typedef void ukf_observationUpdate(float32_t * inState, float32_t * outState, float32_t * noise, float32_t * exogenousInput);

typedef struct ukf_observationModel
{
	arm_matrix_instance_f32 state;

	uint16_t stateDim;

	//Covariance
	arm_matrix_instance_f32 cov;

	//state noise mu
	arm_matrix_instance_f32 mu;

	//propagation function
	ukf_observationUpdate * fx_observationUpdate;

	//Dimension
	uint16_t noiseDim;

	//exogenous input dimension
	uint16_t exoInputDim;

	// magnetometer calibration matrix
	arm_matrix_instance_f32 magC;
	arm_matrix_instance_f32 magM;

} ukf_observationModel;

typedef struct ukf_predictionModel
{
	//State dimension
	uint16_t stateDim;

	//state
	arm_matrix_instance_f32 state;

	//process noise covariance
	arm_matrix_instance_f32 processCov;

	//Process covariance dimension
	uint16_t processCovDim;

	//state noise dimension
	uint16_t observationCovDim;

	//observation noise covariance
	arm_matrix_instance_f32 observationCov;

	//state noise mu
	arm_matrix_instance_f32 observationMu;

	//exogenous input dimension
	uint16_t exoInputDim;

	//observation input
	float32_t * exoInput;

	//CKDF Noise scale factor
	float32_t scaleFactor;
	float32_t scaleFactorSqrt;

	//propagation function
	ukf_predictionUpdate * fx_predictionUpdate;

	/*******************/

	//Sigma points
	uint16_t sigmaPoints;

	//Weight set
	double weightSum0;
	double weightSum1;

	double weightCov0;
	double weightCov1;

	//matrix square root of global coveriance
	arm_matrix_instance_f32 Sx;

	/*Matrix sigma points*/
	arm_matrix_instance_f32 hSzM;

	/*Matrix transposed sigma points*/
	arm_matrix_instance_f32 hSzMT;

	/*Start prediction specific*/
	/*propagated sigma points*/
	arm_matrix_instance_f32 X;

	/*propagated weighted & summed SP*/
	arm_matrix_instance_f32 xh;

	/*new covariance matrix transposed*/
	arm_matrix_instance_f32 Sxn;

	/*new covariance matrix transposed*/
	arm_matrix_instance_f32 SxnT;
	/*end prediction specific*/

	/*Start observation specific*/

	/*Observation propagated points*/
	arm_matrix_instance_f32 Y;

	/*propagated weighted & summed SP*/
	arm_matrix_instance_f32 yh;

	/*new covariance matrix*/
	arm_matrix_instance_f32 Syn;

	/*new covariance matrix transposed*/
	arm_matrix_instance_f32 SynT;

	/*New covariance*/
	arm_matrix_instance_f32 Sy;

	arm_matrix_instance_f32 Syx;
	arm_matrix_instance_f32 SyxT;


	arm_matrix_instance_f32 Syw;
	arm_matrix_instance_f32 Pxy;
	arm_matrix_instance_f32 KG;
	arm_matrix_instance_f32 innov;
	arm_matrix_instance_f32 innovUpdate;


	/*Buffer matrixes*/
	arm_matrix_instance_f32 s1, s2, s3, s4, s5,s5T, D;

} ukf_predictionModel;

int ukf_Init(ukf_predictionModel *model);
int ukf_processPrediction(ukf_predictionModel *model, float32_t * exogenousInput, float32_t dt);
int ukf_processObservation(ukf_predictionModel *predictModel, ukf_observationModel *obsModel, float32_t * exogenousInput);

int setCDKFDiagonalCovariance(arm_matrix_instance_f32 * instance, uint16_t rcDim, float32_t * cov, uint16_t covDim, uint8_t doSqrt);
int ukf_calculatePropagatedCovariance(ukf_predictionModel *model, arm_matrix_instance_f32 * propSigmaPoints, arm_matrix_instance_f32 * calculatedCovariance, uint8_t enableCalcMeasurementPt);
int getTransposedSigmaPoints(ukf_predictionModel * model, arm_matrix_instance_f32 processNoiseCov);
int getSigmaPoints(ukf_predictionModel * model, arm_matrix_instance_f32 processNoiseCov);

#endif
